#include "M0601C.h"

#include <cstdint>

#include <cstdint>



M0601C::M0601C(int motorId, MotorDriver *driver, QObject *parent)
    : QObject(parent), id(motorId), driver(driver), position(0), velocity(0), current(0)
{
    refreshTimer = new QTimer(this);
    connect(refreshTimer, &QTimer::timeout, this, &M0601C::refreshStatus);

    // 监听驱动层返回的响应
    connect(driver, &MotorDriver::responseReceived, this, [=](uint8_t motorId, const QByteArray &response) {
        if (motorId == id) {
            updateStatus(response);
        }
    });

    // 启动定时器
    refreshTimer->start(100); // 每100ms刷新一次状态
}

M0601C::~M0601C()
{
    refreshTimer->stop();
    delete refreshTimer;
}

double M0601C::getPosition() const { return position; }
double M0601C::getVelocity() const { return velocity; }
double M0601C::getCurrent() const { return current; }

void M0601C::setTargetPosition(double targetPosition)
{
    /* 构造设置位置的命令 */
    if (targetPosition < 0 || targetPosition > 360)
    {
        return;
    }
    
    uint8_t command[10] = {0x00};
    uint16_t position = targetPosition / 360.0 * 32767;
    command[0] = id;
    command[1] = 0x64;
    command[2] = position >> 8;
    command[3] = position & 0xFF;
    command[4] = 0x00;
    command[5] = 0x00;
    command[6] = 0x00;
    command[7] = 0x00;
    command[8] = 0x00;
    command[9] = crc8_maxim(command, 9);
    
    driver->sendCommand(id, QByteArray((char*)command, 10));
}

void M0601C::setTargetVelocity(double targetVelocity, double acceleration_time, bool brake)
{
    /* 构造设置速度的命令 */
    if (targetVelocity > 330 || targetVelocity < -330 || acceleration_time < 0.1) 
    {
        return;
    }
    
    uint8_t command[10] = {0x00};
    int16_t velocity = (int16_t)targetVelocity;
    uint8_t acc_time =  acceleration_time * 10; 
    command[0] = id;
    command[1] = 0x64;
    command[2] = velocity >> 8;
    command[3] = velocity & 0xFF;
    command[4] = 0x00;
    command[5] = 0x00;
    command[6] = acc_time;
    command[7] = (brake == true) ? 0xFF : 0x00;
    command[8] = 0x00;
    command[9] = crc8_maxim(command, 9);
    
    driver->sendCommand(id, QByteArray((char*)command, 10));
}

void M0601C::setTargetCurrent(double targetCurrent)
{
    /* 构造设置位置的命令 */
    if (targetCurrent > 8 || targetCurrent < -8)
    {
        return;
    }
    
    uint8_t command[10] = {0x00};
    int16_t current = targetCurrent / 8.0 * 32767;
    command[0] = id;
    command[1] = 0x64;
    command[2] = current >> 8;
    command[3] = current & 0xFF;
    command[4] = 0x00;
    command[5] = 0x00;
    command[6] = 0x00;
    command[7] = 0x00;
    command[8] = 0x00;
    command[9] = crc8_maxim(command, 9);
    
    driver->sendCommand(id, QByteArray((char*)command, 10));
}

void M0601C::updateStatus(const QByteArray &response)
{
    // 根据协议解析响应，更新参数
    // position = /* 从response解析位置 */;
    // velocity = /* 从response解析速度 */;
    // current = /* 从response解析电流 */;

    emit updated(); // 通知参数更新
}

void M0601C::refreshStatus()
{
    // QByteArray command = /* 构造读取状态的命令 */;
    // driver->sendCommand(id, command);
}

uint8_t M0601C::crc8_maxim(const uint8_t *data, uint8_t length)
{
    uint8_t crc = 0x00; // Initial CRC value

    for (uint8_t i = 0; i < length; i++) {
        crc ^= data[i]; // XOR with input byte

        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x80) {
                crc = (crc << 1) ^ 0x31; // Polynomial 0x31 for CRC-8/MAXIM
            } else {
                crc <<= 1;
            }
        }
    }
    return crc;
}

